//------------------------------------------------------------------------------
//  <copyright file="ClientMessages.h" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#pragma once

#include "KukaLBR4PlusControllerExports.h"

#include <Windows.h>
#include <KukaLBR4Definitions.h>

/// <remarks>
/// Contains definitions of messages used to communicate with upstream clients
/// (as opposed to the FRI client which runs on the LBR4+'s controller hardware, which the
/// KukaLBR4PlusController component also communicates with) that want to control
/// or get feedback from the manipulator.  
/// </remarks>


// The structs below will be created from and used to create byte arrays
// so need to ensure that they are packed.
#pragma pack(push, 1)

/// <summary>
/// Payload for message sent by upstream client to set the values of the 
/// machine registers used by the controller state machine.  These are all of the
/// registers for the Kuka LBR4+ controller state machine that can be written to 
/// either by the machine itself or the upstream client.
/// </summary>
struct KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT KukaWorkingRegisterMessage
{
	// Order matters.  Must be in same order (and not skip or add any entries) as "KukaLBR4PlusMachineRegisters" enum.
	// If adding members, make sure to initialize to 0 (or other sane value) in constructor.

	/// <summary>
	/// The controller strategy as defined by KRL (Kuka).
	/// Determines type of control.  Not all command registers
	/// have an effect in all control modes.  
	/// </summary>
	double ControllerStrategyCommandRegister;

	/// <summary> Commanded joint poisitions.  </summary>
	double JointPositionCommandRegister[NUM_JOINTS];

	/// <summary>
	/// Commanded Cartesian positions, represented as first 3 rows of a homogenous transform defining
	/// the target coordinate frame for the end effector.  Last row is assumed to be [0 0 0 1].
	/// </summary>
	double CartesianPositionCommandRegister[CARTESIAN_FRAME_LENGTH];

	/// <summary> Commanded joint torques.  </summary>
	double JointTorqueCommandRegister[NUM_JOINTS];

	/// <summary> Commanded force/torque at the tool control point.  </summary>
	double TCPForceTorqueCommandRegister[CARTESIAN_DIM];

	/// <summary> Commanded joint stiffness.  </summary>
	double JointStiffnessCommandRegister[NUM_JOINTS];

	/// <summary> Commanded joint damping.  </summary>
	double JointDampingCommandRegister[NUM_JOINTS];

	/// <summary> Commanded Cartesian stiffness.  </summary>
	double CartesianStiffnessCommandRegister[CARTESIAN_DIM];

	/// <summary> Commanded Cartesian damping.  </summary>
	double CartesianDampingCommandRegister[CARTESIAN_DIM];

	/// <summary> General purpose scalar.  </summary>
	double Scalar1Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar2Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar3Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar4Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar5Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar6Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar7Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar8Register;

	/// <summary> General purpose 6-dim vector.  </summary>
	double CartVec1Register[CARTESIAN_DIM];

	/// <summary> General purpose 6-dim vector.  </summary>
	double CartVec2Register[CARTESIAN_DIM];

	/// <summary> General purpose 6-dim vector.  </summary>
	double CartVec3Register[CARTESIAN_DIM];

	/// <summary> General purpose vector with dimension equal to number of joints.  </summary>
	double JointVec1Register[NUM_JOINTS];	

	/// <summary> General purpose vector with dimension equal to number of joints.  </summary>
	double JointVec2Register[NUM_JOINTS];

	/// <summary> General purpose vector with dimension equal to number of joints.  </summary>
	double JointVec3Register[NUM_JOINTS];

	/// <summary> First 3 rows of a homogenous transform (last row is assumed [0 0 0 1])
	double Frame1Register[CARTESIAN_FRAME_LENGTH];

	/// <summary> First 3 rows of a homogenous transform (last row is assumed [0 0 0 1])
	double Frame2Register[CARTESIAN_FRAME_LENGTH];

	/// <summary> First 3 rows of a homogenous transform (last row is assumed [0 0 0 1])
	double Frame3Register[CARTESIAN_FRAME_LENGTH];

	/// <summary> Number of the state that the current controller is in. </summary>
	double CurrentControllerStateNumber;

	/// <summary> Initializes a new instance of the <c>KukaWorkingRegisterMessage</c> struct. </summary>
	KukaWorkingRegisterMessage();
};

/// <summary>
/// Payload sent in response to a feedback request from the upstream client. Contains
/// the entire observable state of the controller state machine and the manipulator.
/// Includes all of the machine registers (state machine registers that can be read
/// or written by the machine or the upstream client directly), plus feedback registers
/// that are only set in response to state updates recieved from the FRI client (i.e.
/// the manipulator's controller hardware).
/// </summary>
struct KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT KukaFeedbackMessage
{
	// Order matters.  Must be in same order (and not skip or add any entries) as 
	// the "KukaLBR4PlusWorkingRegisters" enum followed by the "KukaLBR4PlusFeedbackRegisters" enum.

	/// <summary>
	/// The controller strategy as defined by KRL (Kuka).
	/// Determines type of control.  Not all command registers
	/// have an effect in all control modes.  
	/// </summary>
	double ControllerStrategyCommandRegister;

	/// <summary> Commanded joint poisitions.  </summary>
	double JointPositionCommandRegister[NUM_JOINTS];

	/// <summary> Commanded Cartesian poisitions as first 3 rows of homogenous transform.  </summary>
	double CartesianPositionCommandRegister[CARTESIAN_FRAME_LENGTH];

	/// <summary> Commanded joint torques.  </summary>
	double JointTorqueCommandRegister[NUM_JOINTS];

	/// <summary> Commanded force/torque at the tool control point.  </summary>
	double TCPForceTorqueCommandRegister[CARTESIAN_DIM];

	/// <summary> Commanded joint stiffness.  </summary>
	double JointStiffnessCommandRegister[NUM_JOINTS];

	/// <summary> Commanded joint damping.  </summary>
	double JointDampingCommandRegister[NUM_JOINTS];
	
	/// <summary> Commanded Cartesian stiffness.  </summary>
	double CartesianStiffnessCommandRegister[CARTESIAN_DIM];

	/// <summary> Commanded Cartesian damping.  </summary>
	double CartesianDampingCommandRegister[CARTESIAN_DIM];

	/// <summary> General purpose scalar.  </summary>
	double Scalar1Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar2Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar3Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar4Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar5Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar6Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar7Register;

	/// <summary> General purpose scalar.  </summary>
	double Scalar8Register;

	/// <summary> General purpose 6-dim vector.  </summary>
	double CartVec1Register[CARTESIAN_DIM];

	/// <summary> General purpose 6-dim vector.  </summary>
	double CartVec2Register[CARTESIAN_DIM];

	/// <summary> General purpose 6-dim vector.  </summary>
	double CartVec3Register[CARTESIAN_DIM];

	/// <summary> General purpose vector with dimension equal to number of joints.  </summary>
	double JointVec1Register[NUM_JOINTS];

	/// <summary> General purpose vector with dimension equal to number of joints.  </summary>
	double JointVec2Register[NUM_JOINTS];

	/// <summary> General purpose vector with dimension equal to number of joints.  </summary>
	double JointVec3Register[NUM_JOINTS];

	/// <summary> First 3 rows of a homogenous transform (last row is assumed [0 0 0 1])
	double Frame1Register[CARTESIAN_FRAME_LENGTH];

	/// <summary> First 3 rows of a homogenous transform (last row is assumed [0 0 0 1])
	double Frame2Register[CARTESIAN_FRAME_LENGTH];

	/// <summary> First 3 rows of a homogenous transform (last row is assumed [0 0 0 1])
	double Frame3Register[CARTESIAN_FRAME_LENGTH];

	/// <summary> Number of the state that the current controller is in. </summary>
	double CurrentControllerStateNumber;

	/// <summary> Timestamp of the FRI request. </summary>
    double TimestampInMsRegister;

	/// <summary> State of the FRI connection (i.e. Monitor, Command, etc...)  </summary>
	double FriStateRegister;

	/// <summary> Quality of the UDP connection. </summary>
	double UdpQualityRegister;

	/// <summary> Configured rate of FRI requests. </summary>
	double CycleTimeInMsRegister;

	/// <summary> Speed limit (in %) </summary>
	double SafetyLimitInPercentRegister;

	/// <summary> Indicates drive power status. </summary>
	double DrivePowerBitFieldRegister;

	/// <summary> Indicates current controller strategy </summary>
	double ControllerStrategyRegister;

	/// <summary> Joint temperature. </summary>
	double JointTemperatureRegister[NUM_JOINTS];

	/// <summary> The measured joint position. </summary>
	double MeasuredJointPositionRegister[NUM_JOINTS];

	/// <summary> The estimated joint velocity. </summary>
	double MeasuredJointVelocityRegister[NUM_JOINTS];

	/// <summary> Commanded joint position reported by the LBR controller. </summary>
	double ReportedCommandedJointPositionRegister[NUM_JOINTS];

	/// <summary> Commanded joint offset reported by the LBR controller.  </summary>
	double ReportedCommandedJointOffsetRegister[NUM_JOINTS];

	/// <summary> Measured Cartesian position.  </summary>
	double MeasuredCartesianPositionRegister[CARTESIAN_FRAME_LENGTH];

	/// <summary> Commanded cartesian position reported by the LBR controller. </summary>
	double ReportedCommandedCartesianPositionRegister[CARTESIAN_FRAME_LENGTH];

	/// <summary> Commanded cartesian offset reported by the LBR controller. </summary>
	double ReportedCommandedCartesianOffsetRegister[CARTESIAN_FRAME_LENGTH];

	/// <summary> Measured joint torque. </summary>
	double MeasuredJointTorqueRegister[NUM_JOINTS];

	/// <summary> Estimate of torques that is due to external force. </summary>
	double MeasuredExternalJointTorqueRegister[NUM_JOINTS];

	/// <summary> Estimated force/torque applied to tool control point. </summary>
	double EstimatedTCPForceTorqueRegister[CARTESIAN_DIM];

	/// <summary> Current numercial manipulator jacobian. </summary>
	double JacobianRegister[CARTESIAN_DIM * NUM_JOINTS];

	/// <summary> Current numerical mass matrix. </summary>
	double MassMatrixRegister[CARTESIAN_DIM * NUM_JOINTS];

	/// <summary> Torque required for gravity comp in current config. </summary>
	double GravityCompensationRegister[NUM_JOINTS];

	/// <summary> Last joint position command issued over the FRI interface. </summary>
    double LastIssuedJointPositionCommandRegister[NUM_JOINTS];

	/// <summary> Initializes a new instance of the <c>KukaFeedbackMessage</c> struct. </summary>
	KukaFeedbackMessage();
};
#pragma pack(pop)
